{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 通过相机下的图像求解工具坐标系中一点在工具坐标系下的坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import *\n",
    "import cv2\n",
    "from robot import rotx,roty,rotz,tr2rpy,rpy2tr\n",
    "from robot import trotx,troty,trotz,transl,t2r,r2t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_Matrix(string):\n",
    "    m = string+\"_%d%d\"\n",
    "    T = []\n",
    "    for i in range(4):\n",
    "        for j in range(4):\n",
    "            if not i == 3:\n",
    "                T.append(Symbol(m%(i+1,j+1)))\n",
    "            else:\n",
    "                if j != 3:\n",
    "                    T.append(0)\n",
    "                else:\n",
    "                    T.append(1)\n",
    "    return Matrix(T).reshape(4,4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T = get_Matrix('m')\n",
    "T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sympy.abc import a,b,c,x, y, z\n",
    "p = Matrix([x,y,z,1])\n",
    "p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 相机变换\n",
    "P = [[Symbol(\"f_x\"),0,Symbol(\"c_x\"),0],\n",
    "    [0,Symbol(\"f_y\"),Symbol(\"c_y\"),0],\n",
    "     [0,0,1,0]]\n",
    "P=Matrix(P)\n",
    "P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "pc = T*p\n",
    "pc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "P*pc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "M = []\n",
    "tmp = P*T*p\n",
    "M.append(tmp[0] - Symbol(\"u\")*tmp[2])\n",
    "M.append(tmp[1] - Symbol(\"v\")*tmp[2])\n",
    "M = Matrix(M)\n",
    "M"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "collect(M[0,0].expand(),[x,y,z])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "collect(M[1,0].expand(),[x,y,z])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "所以用最小二乘法可以求得，该点在末端坐标系下的位置\n",
    "若不改变位姿，那么新的工具坐标系为 Tool*p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def solve_ps(T_cs,img_pts,cam_mtx):\n",
    "    fx = cam_mtx[0,0]\n",
    "    fy = cam_mtx[1,1]\n",
    "    cx = cam_mtx[0,2]\n",
    "    cy = cam_mtx[1,2]\n",
    "    A = None\n",
    "    B = None\n",
    "    for index,T_c in enumerate(T_cs):\n",
    "        pt = img_pts[index]\n",
    "        u,v = array(pt).flatten()\n",
    "        m11,m12,m13,m14 = [T_c[0,i] for i in range(4)]\n",
    "        m21,m22,m23,m24 = [T_c[1,i] for i in range(4)]\n",
    "        m31,m32,m33,m34 = [T_c[2,i] for i in range(4)]\n",
    "        a11 = cx*m31+fx*m11-m31*u\n",
    "        a12 = cx*m32+fx*m12-m32*u\n",
    "        a13 = cx*m33+fx*m13-m33*u\n",
    "        a21 = cy*m31+fy*m21-m31*v\n",
    "        a22 = cy*m32+fy*m22-m32*v\n",
    "        a23 = cy*m33+fy*m23-m33*v\n",
    "        a   = mat([a11,a12,a13,a21,a22,a23]).reshape((2,3))\n",
    "        b1  = cx*m34+fx*m14-m34*u\n",
    "        b2  = cy*m34+fy*m24-m34*v\n",
    "        b   = -mat([b1,b2]).T\n",
    "        if A is None:\n",
    "            A = a\n",
    "            B = b\n",
    "        else:\n",
    "            A = np.vstack((A,a))\n",
    "            B = np.vstack((B,b))\n",
    "    p = pinv(A)*B\n",
    "    x,y,z = array(p).flatten()\n",
    "    return x,y,z"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 简单验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "p = mat([1.0,2.0,3.0,1.0]).T\n",
    "p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T1 = rpy2tr(0.1,0.2,0.3)*transl(0.1,0.2,100)\n",
    "T1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T2 = rpy2tr(0.3,0.2,0.1)*transl(0.4,0.2,200)\n",
    "T2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "fx = 1511.64442682\n",
    "fy = 1514.72927917\n",
    "cx = 331.982535978\n",
    "cy = 268.613047699\n",
    "mtx = mat(eye(3))\n",
    "mtx[0,0] = fx\n",
    "mtx[1,1] = fy\n",
    "mtx[0,2] = cx\n",
    "mtx[1,2] = cy\n",
    "P = mat(np.zeros((3,4)))\n",
    "P[:3,:3] = mtx\n",
    "Matrix(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "pt1 = P*T1*p\n",
    "pt1 = pt1[:2]/pt1[2]\n",
    "pt1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "pt2 = P*T2*p\n",
    "pt2 = pt2[:2]/pt2[2]\n",
    "pt2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "solve_ps([T1,T2],[pt1,pt2],mtx)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 验证正确性"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 相机参数\n",
    "class Cam(object):\n",
    "    def __init__(self,name=\"0\",mtx=None,dist=None,Tr=None):\n",
    "        self.name = name\n",
    "        self.mtx = self.get_cameraMatrix() if mtx is None else mtx\n",
    "        self.dist = self.get_distCoeffs() if dist is None else dist\n",
    "        if Tr is None:\n",
    "            self.Tr = mat(eye(4))\n",
    "        else:\n",
    "            self.Tr = Tr # 世界坐标系在相机下的表示 Tc_o ,距离单位为相机的单位\n",
    "\n",
    "    def copy(self):\n",
    "        import copy\n",
    "        new = copy.copy(self)\n",
    "        new.Tr = self.Tr.copy()\n",
    "        new.mtx = self.mtx.copy()\n",
    "        new.dist = self.dist.copy()\n",
    "        new.image = self.image.copy()\n",
    "        return new\n",
    "    \n",
    "    def get_cameraMatrix(self):\n",
    "        mtx = np.eye(3)\n",
    "        fx = 1511.64442682\n",
    "        fy = 1514.72927917\n",
    "        cx = 331.982535978\n",
    "        cy = 268.613047699\n",
    "        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = fx,fy,cx,cy\n",
    "        return mtx\n",
    "\n",
    "    def get_distCoeffs(self):\n",
    "        dist = np.zeros((1,5))\n",
    "        k1 = -0.105356293294\n",
    "        k2 = 3.25067090251\n",
    "        p1 = 0.00155905212496\n",
    "        p2 = -0.00569751368371\n",
    "        k3 = -25.6347944753\n",
    "        dist[0] = k1,k2,p1,p2,k3\n",
    "        return dist\n",
    "    \n",
    "    def get_robot_point(self,robot,q,T):\n",
    "        '''\n",
    "        robot: 机器人\n",
    "        q：关节角\n",
    "        T: 附加的机器人工具坐标系，即标记点在机器人末端的位姿\n",
    "        '''\n",
    "        p = robot.fkine(q.T)*T[:,3]\n",
    "        return self.get_p3d_in_img(p[:3,0])\n",
    "    \n",
    "    def get_p3d_in_img(self,p):\n",
    "        '''\n",
    "        世界坐标系下点p在图像中的坐标\n",
    "        '''\n",
    "        pc = self.Tr*np.vstack((p,1))\n",
    "        rvec = cv2.Rodrigues(self.Tr[:3,:3])[0]\n",
    "        tvec = transl(self.Tr)\n",
    "        mtx,dist = self.mtx,self.dist\n",
    "        pt = cv2.projectPoints(p.reshape((1,1,3)),rvec,tvec,mtx,dist)[0] # 投影到相机1\n",
    "        return mat(pt).reshape((2,1))\n",
    "        \n",
    "    def get_real_point(self,pt):\n",
    "        '''\n",
    "        获取图像坐标 pt 经过畸变矫正后的实际图像坐标\n",
    "        '''\n",
    "        mtx,dist = self.mtx,self.dist\n",
    "        real = cv2.undistortPoints(array(pt).reshape((1,1,2)),mtx,dist,np.array([]),mtx) # 畸变矫正\n",
    "        return mat(real).reshape((2,1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sql import Nao\n",
    "# 机器人\n",
    "nao = Nao.get_by_name(\"red\")\n",
    "robot = nao.kin.LeftHand\n",
    "q0 = mat([0.5629360675811768, -0.3141592741012573, -0.8145959377288818, -0.9541060924530029, -0.5369420051574707]).T # 机器人起始点\n",
    "p3d = robot.fkine(q0.T)[:3,3]\n",
    "Tb_c = nao.kin.BottomCamera.fkine([-0.10895586013793945, -0.018450021743774414])\n",
    "\n",
    "# 相机\n",
    "cam = Cam(\"cam\")\n",
    "cam.mtx = nao.bottom_cam.mtx\n",
    "cam.dist = nao.bottom_cam.dist\n",
    "cam.Tr = inv(Tb_c)\n",
    "# 成像坐标\n",
    "#Ts = transl(12,23,34)*rpy2tr(0.1,0.2,0.3) # 标记点在末端工件坐标系的位姿\n",
    "Ts = transl(12,23,34) # 标记点在末端工件坐标系的位姿\n",
    "cam.get_robot_point(robot,q0,Ts)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "num = 10\n",
    "q_list = [q0+np.random.random(q0.shape)*0.2 for i in range(num)]\n",
    "T_cs = [cam.Tr*robot.fkine(q.T) for q in q_list]\n",
    "img_pts = [cam.get_robot_point(robot,q,Ts) for q in q_list]\n",
    "real_img_pts = [cam.get_real_point(pt) for pt in img_pts]\n",
    "solve_ps(T_cs,real_img_pts,cam.mtx)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
